// #pragma once
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <vector>
#include "rosconnector.h"




//一定不能用float来计算矩阵！！！否则不会收敛！！！
#ifndef Pi
#define Pi 3.1415926
#endif
#ifndef ZInCam
#define ZInCam 0.03                    ////
#endif
typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatXf;
// 求矩阵的伪逆
MatXf pinv(MatXf x);

// 像素类
class Luminance
{
public:
	float x, y;   // 点坐标 (米)
	float Ix, Iy; // 梯度
};
// 光度视觉伺服类
class VS_photometric {
public:
	 Eigen::Matrix<float, 6, 1> cameraVel;
	Eigen::Matrix<double, 6, 1> norm_cameraVel;
	std::vector<Eigen::Matrix<float, 6, 1>> allCameraVel;
	std::vector<double> allErrorMean;
	///////////////////////////////////////////////////////////////////////////////ROS*  connector;
	//cv::Mat desImgGray;
private:
	// 相机内参
	float focalLengthPIX, u0, v0;
	cv::Mat desImg;
	cv::Mat desImgGray;
	cv::Mat descutimg;
	unsigned int bord1,bord2; //10
	// 使用像素点的数量
	unsigned int pixelNum;
	// 像素类的数组
	Luminance *pixInfo;
	Eigen::MatrixXd Lsd;
	Eigen::MatrixXd Hsd;
	Eigen::MatrixXd diagHsd;
public:
	VS_photometric(cv::Mat desImg_, float focalLengthPIX_, float u0_, float v0_);
	~VS_photometric();
	void servoing(cv::Mat camGetImage);
	void showerror(cv::Mat camGetImage);
	void plotcameraVel();
	void VScontrol();
	void VScontrol_simulation();
private:
	// 像素坐标系转化为相机坐标系下归一化坐标值
	void convertPoint(const double &u, const double &v, double &x, double &y) {
		x = (u - u0) / focalLengthPIX;
		y = (v - v0) / focalLengthPIX;
	}


	float derivativeFilterX(cv::Mat &I, const unsigned int r, const unsigned int c) {
		return (2047.0 * (I.at<uchar>(r, c + 1) - I.at<uchar>(r, c - 1)) +
			913.0 * (I.at<uchar>(r, c + 2) - I.at<uchar>(r, c - 2)) +
			112.0 * (I.at<uchar>(r, c + 3) - I.at<uchar>(r, c - 3))
			+ 1241 * (I.at<uchar>(r + 1, c + 1) - I.at<uchar>(r - 1, c - 1) + I.at<uchar>(r - 1, c + 1) - I.at<uchar>(r + 1, c - 1))
			+ 554 * (I.at<uchar>(r + 1, c + 2) - I.at<uchar>(r - 1, c - 2) + I.at<uchar>(r - 1, c + 2) - I.at<uchar>(r + 1, c - 2))
			+ 277 * (I.at<uchar>(r + 2, c + 1) - I.at<uchar>(r - 2, c - 1) + I.at<uchar>(r - 2, c + 1) - I.at<uchar>(r + 2, c - 1))
			+ 124 * (I.at<uchar>(r + 2, c + 2) - I.at<uchar>(r - 2, c - 2) + I.at<uchar>(r - 2, c + 2) - I.at<uchar>(r + 2, c - 2))) /
			19914.0;
	}

	float derivativeFilterY(cv::Mat &I, const unsigned int r, const unsigned int c) {
		return (2047.0 * (I.at<uchar>(r + 1, c) - I.at<uchar>(r - 1, c)) +
			913.0 * (I.at<uchar>(r + 2, c) - I.at<uchar>(r - 2, c)) +
			112.0 * (I.at<uchar>(r + 3, c) - I.at<uchar>(r - 3, c))
			+ 1241 * (I.at<uchar>(r + 1, c + 1) - I.at<uchar>(r - 1, c - 1) + I.at<uchar>(r + 1, c - 1) - I.at<uchar>(r - 1, c + 1))
			+ 554 * (I.at<uchar>(r + 2, c + 1) - I.at<uchar>(r - 2, c - 1) + I.at<uchar>(r + 2, c - 1) - I.at<uchar>(r - 2, c + 1))
			+ 277 * (I.at<uchar>(r + 1, c + 2) - I.at<uchar>(r - 1, c - 2) + I.at<uchar>(r + 1, c - 2) - I.at<uchar>(r - 1, c + 2))
			+ 124 * (I.at<uchar>(r + 2, c + 2) - I.at<uchar>(r - 2, c - 2) + I.at<uchar>(r + 2, c - 2) - I.at<uchar>(r - 2, c + 2))) /
			19914.0;
	}
	// // 计算interaction矩阵
	void getInteraction();
};